/**
*  \file cansim.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "cansim.h"
#include "../assertor/assertor.h"
#include "../cant/cant.h"
#include "../app/app.h"
#include "../sched/sched.h"
#include "../mcu/mcu.h"
#include "../endian/endian_read.h"
#include "../nth/nth.h"
#include "../saturated/sinc.h"
#include "../endian/endian_read.h"
#include "../can/can.h"
#include "../output/output.h"
#include "../endian/endian_read.h"
#include "../endian/endian_write.h"
#include <stdio.h>
/*===============================================[ private defines   ]================================================*/

/*===============================================[ private datatypes ]================================================*/

/** Private variables */
struct cansim_private
{
   struct tasklet task_10ms;
   U16 output_timer;
};

/*===============================================[ private variables ]================================================*/
static struct cansim_private cansim;

/*===============================================[ private functions ]================================================*/

static void cansim_10ms_service (struct tasklet *task);
static void cansim_send_outputs(void);
//static void cansim_print_msg(struct canmsg *msg);
static void sim_digital_input(struct canmsg* msg, void *data );
static void sim_analog_input_x(struct canmsg* msg, void *data );


/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/

RC cansim_initialize( void )
{
   pal_memzero( &cansim, sizeof cansim);

   tasklet_assign( sched_get_slice_10ms(), &cansim.task_10ms,  cansim_10ms_service, NIL );

   can_install_rx_callback( can_channel_sim, canid_ext_sim_digital_input_0,sim_digital_input,  (void*) 0 );
   can_install_rx_callback( can_channel_sim, canid_ext_sim_digital_input_1,sim_digital_input,  (void*) 4 );
   can_install_rx_callback( can_channel_sim, canid_ext_sim_analog_input_0, sim_analog_input_x, (void*) 0  );
   can_install_rx_callback( can_channel_sim, canid_ext_sim_analog_input_1, sim_analog_input_x, (void*) 4  );
   can_install_rx_callback( can_channel_sim, canid_ext_sim_analog_input_2, sim_analog_input_x, (void*) 8  );
   can_install_rx_callback( can_channel_sim, canid_ext_sim_analog_input_3, sim_analog_input_x, (void*) 12 );
   can_install_rx_callback( can_channel_sim, canid_ext_sim_analog_input_4, sim_analog_input_x, (void*) 16 );
  // can_install_rx_callback( can_channel1, canid_ext_sim_analog_input_5, sim_analog_input_x, (void*) 20 );

   return RC_SUCCESS;
}




/*===============================================[ private functions ]================================================*/

static void cansim_10ms_service (struct tasklet *task)
{
   UNREFERENCED( task );

   cansim_send_outputs();

}

static void sim_digital_input(struct canmsg* msg, void *data )
{
   U32 index=(U32)data;
   U32 count=0;
   for( count=0; count<4; count++ )
   {
	   mcu_set_port_sim( index+count,  endian_read_u16_little( &canmsg_data_ptr(msg)[count*2]) );
   }
}

static void sim_analog_input_x(struct canmsg* msg, void *data )
{
   U32 index=(U32)data;
   U32 count=0;
   for( count=0; count<4; count++ )
   {
	   mcu_set_adc_sim( index+count,  endian_read_u16_little( &canmsg_data_ptr(msg)[count*2]) );
   }

}

#if 0
static void cansim_print_msg(struct canmsg *msg)
{
	U8 count;
	printf("0x%03X %i [",canmsg_get_id( msg ),canmsg_get_dlc( msg ));
	for(count=0;count<canmsg_get_dlc( msg );count++)
	{
		printf(" 0x%02X",canmsg_data_ptr( msg )[count]);
		if( count+1 < canmsg_get_dlc( msg ))
		{
			printf(",");
		}
		else
		{
			printf(" ");
		}
	}
	printf("]\n");
}
#endif


static void cansim_send_outputs(void)
{ /* send Outputs*/

	{
		struct canmsg msg;
		U8 *data = canmsg_set( &msg, canid_ext_sim_digital_output_0, 8);
		endian_write_u16_little(&data[0], mcu_get_port_out_sim(0 ) ) ;
		endian_write_u16_little(&data[2], mcu_get_port_out_sim(1 ) ) ;
		endian_write_u16_little(&data[4], mcu_get_port_out_sim(2 ) ) ;
		endian_write_u16_little(&data[6], mcu_get_port_out_sim(3 ) ) ;
		can_tx_msg( can_channel_sim, &msg );
	}

	{
		struct canmsg msg;
		U8 *data = canmsg_set( &msg, canid_ext_sim_digital_output_1, 8);
		endian_write_u16_little(&data[0], mcu_get_port_out_sim(4 ) ) ;
		endian_write_u16_little(&data[2], mcu_get_port_out_sim(5 ) ) ;
		can_tx_msg( can_channel_sim, &msg );
	}

	{
		struct canmsg msg;
		U8 *data = canmsg_set( &msg, canid_ext_sim_pwm_output, 8);
		endian_write_u16_little(&data[0],output_get_current_data()->main_contactor_pwm);
		endian_write_u16_little(&data[2],output_get_current_data()->iso_valve_pwm);
		can_tx_msg( can_channel_sim, &msg );
	}
}

/***********************************************************************************************************************
*                             Copyright 2018 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
